#include "QDoc.h"

#include <vector>
#include <fstream>
#include <iostream>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>

#include "pcdlaplace.h"

#include "lapackUtil.h"

extern "C" {
#include "blaswrap.h"
  #include "f2c.h"
  #include "clapack.h"

  int dgemv_(char *, integer *, integer *, 
    doublereal *, doublereal *, integer *, doublereal *, integer *, 
    doublereal *, doublereal *, integer *);
}

#ifdef _DEBUG

#pragma comment (lib, "clapackd.lib")
#pragma comment (lib, "blasd.lib")
#pragma comment (lib, "cblaswrapd.lib")
#pragma comment (lib, "f77blaswrapd.lib")
#pragma comment (lib, "libf2cd.lib")
#pragma comment (lib, "tmglibd.lib")

#else

#pragma comment (lib, "clapack.lib")
#pragma comment (lib, "blas.lib")
#pragma comment (lib, "cblaswrap.lib")
#pragma comment (lib, "f77blaswrap.lib")
#pragma comment (lib, "libf2c.lib")
#pragma comment (lib, "tmglib.lib")

#endif




QDoc::QDoc(void)
{
}

QDoc::~QDoc(void)
{
}

bool QDoc::openFile(const char *filename) {
	std::ifstream ifs;
    std::string line;

    std::string s;
    std::vector<std::string> stvec;

	std::vector<double> pt;

    ifs.open(filename);

	double x, y, z, val;
    

    if (ifs.is_open()) {
		m_points.clear();
		pt.clear();

		std::getline(ifs, line);
		while(line.length()>0) {
			boost::algorithm::split(stvec, line, boost::algorithm::is_any_of(" \n"));

			if(stvec.size()<4) {
				continue;
			}
			
            try{
				if(stvec[0]=="Vertex"){
					x = boost::lexical_cast<double>(stvec[2]);
					y = boost::lexical_cast<double>(stvec[3]);
					z = boost::lexical_cast<double>(stvec[4]);
					if(stvec.size()>5) {
						val = boost::lexical_cast<double>(stvec[5]);
					}
					
					pt.clear();

					pt.push_back(x);
					pt.push_back(y);
					pt.push_back(z);
					pt.push_back(val);

					m_points.push_back(pt);
				}
            }
            catch(boost::bad_lexical_cast&) {
                printf("lexical_cast fail\n");
            }

			if(ifs.eof()) {
				break;
			}
			std::getline(ifs, line);
		}
    }

    return true;
}

void QDoc::cal_laplace() {
	if(m_points.size() >10) {
		double x,y,z;
		double avg_edge = 0.35;
		PCloud pc;
		VPCloud* vpc;
		dPoint* dpoint;

		std::vector<double> pt;
		BOOST_FOREACH(pt, m_points) {
			x=pt[0];
			y=pt[1];
			z=pt[2];


			dpoint = new dPoint(x,y,z, (double)1);
			vpc = new VPCloud();
			vpc->set_coord(*dpoint);

			pc.add_point(*vpc);
		}

		generate_laplace_matrix_sparse_matlab_dim3(
			pc, avg_edge, avg_edge, 
			m_II, m_JJ, m_SS);


	}
}


std::vector<double>  QDoc::multiplyMatrix(std::vector<double> param) {
	std::vector<double> result;

		/*
		int dgemv_(char *trans, integer *m, integer *n, doublereal *
	alpha, doublereal *a, integer *lda, doublereal *x, integer *incx, 
	doublereal *beta, doublereal *y, integer *incy)
			*/
	if(m_II.size()<=0) {
		cal_laplace();
	}

	long int size = m_points.size(), inc=1;
	double *A = new double[size*size];
	double *xarray = new double[size];
	double *yarray = new double[size];
	double alpha = 1, beta = 0;
	int i, j;
	std::vector<double> rt;
	int lapack_result;
	
	memset(A, 0, sizeof(double)*size*size);

	convertIJS2colMatrix(m_II, m_JJ, m_SS, A, size, size);
	convertVector2Array(param, xarray);

	lapack_result = dgemv_("N", 
			&size,	//m
			&size,	//n
			&alpha,	// alpha
			A, 
			&size,	//lda
			xarray,	// x
			&inc,	// incx
			&beta,	//beta
			yarray,	// y
			&inc);			//incy
	
	for(i=0; i<size; i++) {
		result.push_back(yarray[i]);
	}

	delete[] A;
	delete[] xarray;
	delete[] yarray;
		


	return result;
}
